////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "Grid.hpp"

#include <SFML/Graphics/Image.hpp>

#ifdef _DEBUG
#include <SFML/Graphics.hpp>
#endif

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace ai {

Grid::Grid( unsigned _uWidth, unsigned _uHeight, unsigned _uCellSize ) :
    m_Cells( _uWidth / _uCellSize, _uHeight / _uCellSize ),
    m_uCellSize( _uCellSize ),
    m_uNumCells( m_Cells.x * m_Cells.y ),
    m_pGrid( new bool[m_uNumCells] )
{
    memset( m_pGrid, false, m_Cells.x * m_Cells.y );
}


Grid::~Grid()
{
    delete[] m_pGrid;
}

////////////////////////////////////////////////////////////
/// Load the grid from an image file
////////////////////////////////////////////////////////////
void Grid::LoadFromImage( const std::string& _Filename ) {
    sf::Image imageGrid;

    if ( imageGrid.loadFromFile( _Filename ) ) {
        sf::Vector2u imgSize( imageGrid.getSize() );
        unsigned x = 0, 
            y = 0;
        for ( unsigned i = 0; i < imgSize.x; i += m_uCellSize, ++x) {
            for ( unsigned j = 0; j < imgSize.y; j += m_uCellSize, ++y) {
                if ( IsValid( x, y ) ) {
                    if ( imageGrid.getPixel( i, j ).r > 0 ) {
                        m_pGrid[ x + m_Cells.x * y ] = true;
                    } else {
                        m_pGrid[ x + m_Cells.x * y ] = false;
                    }
                }
            }
        }
    }
}

////////////////////////////////////////////////////////////
/// Mark a point as blocked/unblocked
////////////////////////////////////////////////////////////
bool Grid::IsBlocked( int _iX, int _iY, bool _bNeedConvert ) const {
    if ( _bNeedConvert ) {
        sf::Vector2i gridPoint;
        ConvertToGridSpace( _iX, _iY, gridPoint );
        _iX = gridPoint.x;
        _iY = gridPoint.y;
    }

    if ( IsValid( _iX, _iY ) ) {
        return m_pGrid[ _iX + _iY * m_Cells.x ];
    } else {
        return true;
    }    
}

void Grid::MarkAsBlocked( int _iX, int _iY, bool _bNeedConvert ) {
    if ( _bNeedConvert ) {
        sf::Vector2i gridPoint;
        ConvertToGridSpace( _iX, _iY, gridPoint );
        _iX = gridPoint.x;
        _iY = gridPoint.y;
    }
    
    if ( IsValid( _iX, _iY ) ) {
        m_pGrid[ _iX + _iY * m_Cells.x ] = true;
    }
}

void Grid::MarkAsUnblocked( int _iX, int _iY, bool _bNeedConvert ) {
    if ( _bNeedConvert ) {
        sf::Vector2i gridPoint;
        ConvertToGridSpace( _iX, _iY, gridPoint );
        _iX = gridPoint.x;
        _iY = gridPoint.y;
    }
    
    if ( IsValid( _iX, _iY ) ) {
        m_pGrid[ _iX + _iY * m_Cells.x ] = false;
    }
}

void Grid::ToggleObstacle( int _iX, int _iY, bool _bNeedConvert ) {
    if ( _bNeedConvert ) {
        sf::Vector2i gridPoint;
        ConvertToGridSpace( _iX, _iY, gridPoint );
        _iX = gridPoint.x;
        _iY = gridPoint.y;
    }
    
    if ( IsValid( _iX, _iY ) ) {
        unsigned uPoint = _iX + _iY * m_Cells.x;
        m_pGrid[uPoint] = !m_pGrid[uPoint];
    }
}

void Grid::RemoveAllObstacles() {
    for ( int i = 0; i < m_Cells.x; ++i) {
        for ( int j = 0; j < m_Cells.y; ++j) {
            m_pGrid[ i + j * m_Cells.x ] = false;
        }
    }
}

////////////////////////////////////////////////////////////
/// Convert a point from grid to world/world to grid space
////////////////////////////////////////////////////////////
void Grid::ConvertToGridSpace( int _iX, int _iY, sf::Vector2i& _OutPoint ) const {
    _OutPoint.x = _iX / m_uCellSize;
    _OutPoint.y = _iY / m_uCellSize;
}

void Grid::ConvertToWorldSpace( int _iX, int _iY, sf::Vector2f& _OutPoint ) const {
    _OutPoint.x = (float)_iX * m_uCellSize;
    _OutPoint.y = (float)_iY * m_uCellSize;
}

/*********************** Accessors ************************/
bool Grid::IsValid( int _iX, int _iY ) const {
    return ( _iX >= 0 && _iX < m_Cells.x &&
        _iY >= 0 && _iY < m_Cells.y );
}

#ifdef _DEBUG
void Grid::DebugRender( sf::RenderWindow* _pRenderWindow, bool _bDisplayObstacles ) {
    if ( _bDisplayObstacles ) {
        sf::RectangleShape rectObst( sf::Vector2f( (float)m_uCellSize, (float)m_uCellSize ) );
        sf::Vector2f spacePoint;

        for ( int i = 0; i < m_Cells.x; ++i) {
            for ( int j = 0; j < m_Cells.y; ++j) {
                if ( m_pGrid[ i + j * m_Cells.x ] ) {
                    ConvertToWorldSpace( i, j, spacePoint );
                    rectObst.setPosition( spacePoint );
                    _pRenderWindow->draw( rectObst );
                }
            }
        }
    }

    if ( m_uCellSize > 1 ) {
        sf::Vertex lines[2] = {
            sf::Vertex( sf::Vector2f( 0.0f, 0.0f ), sf::Color(0,120,120) ),
            sf::Vertex( sf::Vector2f( (float)m_Cells.x * m_uCellSize, 0.0f ), sf::Color(0,120,120) ),
        };
        
        // Horizontal lines
        for ( int i = 1; i < m_Cells.y; ++i ) {
            lines[0].position.y = i * (float)m_uCellSize;
            lines[1].position.y = i * (float)m_uCellSize;

            _pRenderWindow->draw( lines, 2, sf::Lines );
        }

        // Vertical lines
        lines[0].position.y = 0.0f;
        lines[1].position.y = (float)m_Cells.x * m_uCellSize;
        for ( int i = 1; i < m_Cells.x; ++i ) {
            lines[0].position.x = i * (float)m_uCellSize;
            lines[1].position.x = i * (float)m_uCellSize;
            
            _pRenderWindow->draw( lines, 2, sf::Lines );
        }
    }
}
#endif

    } // namespace ai
} // namespace smeg